//
// Created by jyh on 2021/8/9.
//

#include "StreamObs.h"
#include "Rnxeph.h"
void RtcmObsDecoder_Initial(struct RtcmObsDecoder *Decoder) {
    Decoder->nbyte = Decoder->len = 0;
    memset(Decoder->buff,0,sizeof(Decoder->buff));
    init_rtcm(&Decoder->m_rtcm);
    memset(Decoder->m_rtcm.nav.glo_fcn,0,sizeof(Decoder->m_rtcm.nav.glo_fcn)); // It's better to add this into init_rtcm, or it will cause some INVALID WRITE. jyh
}

int StreamReader(char *buff, int nbyte, double stax[3], int mjd, double sod) {
    int ret;

    // Read EPH
    static struct RtcmObsDecoder Decoder;
    static initDecoder=0; // 0-initial Decoder; 1-hold Decoder
    double ep[6], sow;
    int week;

    if (!initDecoder)
        RtcmObsDecoder_Initial(&Decoder);
    initDecoder=1;

    for (int i = 0; i < nbyte; i++) {
        ret = input_rtcm3(&Decoder.m_rtcm, buff[i]);
        if(ret == 5){
            printf("*****WARNING(StreamObs.c): Not support antenna!\n");
        }
        if(ret == 1){
            printf("*****WARNING(StreamObs.c): Not support observation!\n");
        }
        if(ret == 2){
            // rtklib: rtcm->time, if no time in buff, get cpu time.
            if(mjd==0 && sod==0){
                memset(ep,0,sizeof(ep));
                time2epoch(Decoder.m_rtcm.time,ep);
                sow = time2gpst(Decoder.m_rtcm.time,&week);
                wksow2mjd(week,sow,&DLY.mjd,&DLY.sod);
            }else{
                DLY.mjd=mjd;
                DLY.sod=sod;
            }
            StreamObs_m_setEphData(&Decoder.m_rtcm.nav,Decoder.m_rtcm.ephsat);
            // if stax!=0, retrun sat location
            // if((fabs(stax[0])+fabs(stax[1])+fabs(stax[2]))>1e-8){
            //     memcpy(DLY.SIT[0].x,stax,sizeof(double )*3);
            //     replySat0();
            // }
        }
        if(ret>0){
            free_rtcm(&Decoder.m_rtcm);
            init_rtcm(&Decoder.m_rtcm);
        }

    }
    return 0;
}
char StreamObs_m_setEphData(nav_t* nav,int psat){
    int isat,isys,mjd,week,index;
    char prn[4] = {0};
    double sow,sod;
    satno2id(psat, prn);
    if (!(strlen(prn)) || -1 == (isys = index_string(SYS, prn[0]))
        || -1 == (isat = pointer_string(DLY.nprn, DLY.cprn, prn))) {
        return ' ';
    }
    if (SYS[isys] != 'R') {
        struct GPSEPH ephg;
        memset(&ephg,0,sizeof(struct GPSEPH));
        ephg.dt=1e6;
        //Rnxeph_GPSEPH_initial(&ephg);
        if(SYS[isys] == 'C'){
            nav->eph[psat - 1].toc.time -= 14; /*change it into bdt*/
            nav->eph[psat - 1].toe.time -= 14;
        }
        sow = time2gpst(nav->eph[psat - 1].toc, &week); /***Time in GPST****/
        wksow2mjd(week, sow, &mjd, &sod);
        strcpy(ephg.cprn, prn);
        ephg.mjd = mjd;                          //Time in GPS
        ephg.sod = sod;
        ephg.a0 = nav->eph[psat - 1].f0;
        ephg.a1 = nav->eph[psat - 1].f1;
        ephg.a2 = nav->eph[psat - 1].f2;
        ephg.aode = nav->eph[psat - 1].iode;
        ephg.crs = nav->eph[psat - 1].crs;
        ephg.dn = nav->eph[psat - 1].deln;
        ephg.m0 = nav->eph[psat - 1].M0;
        ephg.cuc = nav->eph[psat - 1].cuc;
        ephg.e = nav->eph[psat - 1].e;
        ephg.cus = nav->eph[psat - 1].cus;
        ephg.roota = sqrt(nav->eph[psat - 1].A);
        ephg.cic = nav->eph[psat - 1].cic;
        ephg.omega0 = nav->eph[psat - 1].OMG0;
        ephg.cis = nav->eph[psat - 1].cis;
        ephg.i0 = nav->eph[psat - 1].i0;
        ephg.crc = nav->eph[psat - 1].crc;
        ephg.omega = nav->eph[psat - 1].omg;
        ephg.omegadot = nav->eph[psat - 1].OMGd;
        ephg.idot = nav->eph[psat - 1].idot;
        ephg.resvd0 = nav->eph[psat - 1].code;
        ephg.resvd1 = nav->eph[psat - 1].flag;
        ephg.accu = nav->eph[psat - 1].sva;
        ephg.hlth = nav->eph[psat - 1].svh;
        ephg.aodc = nav->eph[psat - 1].iodc;
        ephg.tgd = nav->eph[psat - 1].tgd[0];
        ephg.tgd1 = nav->eph[psat - 1].tgd[1];
        /*bdtime toe & week*/
        ephg.toe = time2gpst(nav->eph[psat-1].toe,&week);
        ephg.week = week;
        //	memcpy(ephg.tgd,nav->eph[isat-1].tgd,sizeof(double)*4);
        ephg.aode = genAode(SYS[isys],ephg.mjd,ephg.sod,ephg.toe,ephg.aode,&ephg);

        // store to BRDEPH
        index=map_gpseph_find(BRDEPH.gnsseph, ephg.cprn);
        if(index==-1){
            printf("*****WARNING(StreamObs.c): %s is not used in session.obj!\n", ephg.cprn);
        }else{
            memset(&BRDEPH.gnsseph[index],0,sizeof(struct GPSEPH));
            BRDEPH.gnsseph[index]=ephg;
        }
        // BRDEPH.gnsseph.m_type = SYS[isys];
    }else{
        struct GLSEPH ephr;
        memset(&ephr,0,sizeof(struct GLSEPH));
        ephr.dt=1e6;
        //Rnxeph_GLSEPH_initial(&ephr);
        int week;
        psat = atoi(prn + 1);
        strcpy(ephr.cprn,prn);
        memcpy(ephr.pos,nav->geph[psat-1].pos,sizeof(double) * 3);
        memcpy(ephr.vel,nav->geph[psat-1].vel,sizeof(double) * 3);
        memcpy(ephr.acc,nav->geph[psat-1].acc,sizeof(double) * 3);
        for(int i = 0;i < 3;i++){
            ephr.pos[i] /= 1e3;
            ephr.vel[i] /= 1e3;
            ephr.acc[i] /= 1e3;
        }
        ephr.frenum = nav->geph[psat-1].frq;
        ephr.gamma = nav->geph[psat-1].gamn;
        ephr.tau = -nav->geph[psat-1].taun;
        ephr.health = nav->geph[psat-1].svh;
        ephr.age = nav->geph[psat-1].age;
        gtime_t tk = gpst2utc(nav->geph[psat-1].tof); /*change it back into utc*/
        ephr.tk = time2gpst(tk,&week);
        double sow=time2gpst(nav->geph[psat-1].toe,&week);/*GPST*/
        wksow2mjd(week,sow,&ephr.mjd,&ephr.sod);
        ephr.aode = nav->geph[psat - 1].iode;
        ephr.aode = genAode(SYS[isys], ephr.mjd, ephr.sod, 0.0, ephr.aode, NULL);

        // store to BRDEPH
        index=map_glseph_find(BRDEPH.glseph, ephr.cprn);
        if(index==-1){
            printf("*****WARNING(StreamObs.c): %s is not used in session.obj!\n", ephr.cprn);
        }else{
            memset(&BRDEPH.glseph[index],0,sizeof(struct GLSEPH));
            BRDEPH.glseph[index]=ephr;
        }
    }
    return SYS[isys];
}

